// Copyright 2022 Chen Jun
// Licensed under the MIT License.

#ifndef ARMOR_DETECTOR_PNP_SOLVER_HPP_
#define ARMOR_DETECTOR_PNP_SOLVER_HPP_
#include "Compensator.h"
#include "DataType.hpp"
#include "angles.h"
#include "armor.hpp"
#include "easylogging++.h"
#include "opencv2/opencv.hpp"
#include "tracker.hpp"
#include <Eigen/Eigen>
#include <array>
#include <cmath>
#include <cstdlib>
#include <filesystem>
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <regex>
#include <vector>


namespace hnurm
{
    class TSolver
    {
    public:

        TSolver(const cv::FileNode &cfg_node, const std::string &camera_id)
        {
            /* 坐标轴方向：
             * x轴：->
             * z轴：↑
             * 原点位置：装甲板中心
             */
            small_armor_points_ = {
                    cv::Point3f(-SMALL_ARMOR_WIDTH / 2, 0, SMALL_ARMOR_HEIGHT / 2),
                    cv::Point3f(-SMALL_ARMOR_WIDTH / 2, 0, -SMALL_ARMOR_HEIGHT / 2),
                    cv::Point3f(SMALL_ARMOR_WIDTH / 2, 0, -SMALL_ARMOR_HEIGHT / 2),
                    cv::Point3f(SMALL_ARMOR_WIDTH / 2, 0, SMALL_ARMOR_HEIGHT / 2),

                    cv::Point3f(0 , 0 , SMALL_ARMOR_HEIGHT / 2),
                    cv::Point3f(-SMALL_ARMOR_WIDTH / 2 , 0 , 0),
                    cv::Point3f(0 , 0 , -SMALL_ARMOR_HEIGHT / 2),
                    cv::Point3f(SMALL_ARMOR_WIDTH / 2, 0, 0),

                    cv::Point3f(0, 0, 0),

            };


            large_armor_points_ = {
                    cv::Point3f(-LARGE_ARMOR_WIDTH / 2, 0, LARGE_ARMOR_HEIGHT / 2),
                    cv::Point3f(-LARGE_ARMOR_WIDTH / 2, 0, -LARGE_ARMOR_HEIGHT / 2),
                    cv::Point3f(LARGE_ARMOR_WIDTH / 2, 0, -LARGE_ARMOR_HEIGHT / 2),
                    cv::Point3f(LARGE_ARMOR_WIDTH / 2, 0, LARGE_ARMOR_HEIGHT / 2),

                    cv::Point3f(0 , 0 , LARGE_ARMOR_WIDTH / 2),
                    cv::Point3f(-LARGE_ARMOR_WIDTH / 2 , 0 , 0),
                    cv::Point3f(0 , 0 , -LARGE_ARMOR_WIDTH / 2),
                    cv::Point3f(LARGE_ARMOR_WIDTH / 2, 0, 0),

                    cv::Point3f(0, 0, 0),

            };

            buff_armor_points_ = {
                cv::Point3f(-BUFF_ARMOR_WIDTH / 2, 0, BUFF_ARMOR_HEIGHT / 2),
                cv::Point3f(-BUFF_ARMOR_WIDTH / 2, 0, -BUFF_ARMOR_HEIGHT / 2),
                cv::Point3f(BUFF_ARMOR_WIDTH / 2, 0, -BUFF_ARMOR_HEIGHT / 2),
                cv::Point3f(BUFF_ARMOR_WIDTH / 2, 0, BUFF_ARMOR_HEIGHT / 2),

                cv::Point3f(0, 0, 0),
            };


            // 从配置文件中读取相机偏移值
            float cam_bias_z, cam_bias_y;
            cfg_node["camera_bias_z"] >> cam_bias_z;
            cfg_node["camera_bias_y"] >> cam_bias_y;

            // 将相机偏移值存入向量
            _cam_bias << cam_bias_z, cam_bias_y, 0;

            // 从配置文件中读取校准信息路径
            std::string calib_info_path;
            cfg_node["calib_info_path"] >> calib_info_path;

            // 拼接路径，动态识别校准信息文件类型
            for (const auto& entry : std::filesystem::directory_iterator(calib_info_path)) {
                if (entry.is_regular_file()) {
                    // 使用 filename() 成员函数获取文件名
                    std::string file_stem = entry.path().stem().string();
                    std::string file_name = entry.path().filename().string();
                    if (file_stem == camera_id)
                    {
                        calib_info_path += file_name;
                        break;
                    }
                }
            }

            // 如果相机id为空，则表示没有使用相机，随便打开一个校准文件
            if (camera_id.empty()){
                calib_info_path  += "2BDF67015844.yaml";
            }

            // 打开校准信息文件
            cv::FileStorage calib_info(calib_info_path, cv::FileStorage::READ);

            // 若校准信息文件未打开成功，则输出错误信息
            if (!calib_info.isOpened())
            {
                CLOG(INFO, "camera") << "Failed to load calib info in " << calib_info_path;
            }

            // 从校准信息文件中读取相机矩阵和畸变系数
            calib_info["camera_matrix"] >> camera_matrix_;
            calib_info["distortion_coefficients"] >> dist_coeffs_;
        }

        /**
         * @brief 通过solvePnP函数求解PnP问题，得到装甲板在世界坐标系下的姿态信息
         * @param armor Armor对象，表示装甲板
         * @param g_pitch 摄像头的pitch角度
         * @param g_yaw 摄像头的yaw角度
         * @param dt 时间间隔
         * @return 是否成功获取到姿态信息
         */
        bool GetTransformation(Armor &armor, float g_pitch, float g_yaw, const double &dt)
        {   // 求解PnP问题

            //没有接串口时候，就如下假设
            //g_pitch = 0;
            //g_yaw = 0;


            cv::Mat rvec, tvec;
            if (armor.armor_type == ArmorType::SMALL)
                cv::solvePnP(small_armor_points_,
                             armor.points2d,
                             camera_matrix_,
                             dist_coeffs_,
                             rvec,
                             tvec,
                             false,
                             cv::SOLVEPNP_IPPE);
            else if (armor.armor_type == ArmorType::LARGE)
                cv::solvePnP(large_armor_points_,
                             armor.points2d,
                             camera_matrix_,
                             dist_coeffs_,
                             rvec,
                             tvec,
                             false,
                             cv::SOLVEPNP_IPPE);

            // 构造变换矩阵
            armor._translation << tvec.at<double>(0, 0), tvec.at<double>(1, 0), tvec.at<double>(2, 0);
            Eigen::AngleAxisd rot_angle(cv::norm(rvec),
                                        Eigen::Vector3d(rvec.at<double>(0, 0),
                                                        rvec.at<double>(1, 0),
                                                        rvec.at<double>(2, 0)));
            armor._rmat = rot_angle.matrix();
            float sp = std::sin(g_pitch), cp = std::cos(g_pitch), sy = std::sin(g_yaw), cy = std::cos(g_yaw);
            armor._r1 << 1, 0, 0,
                    0, 0, 1,
                    0, -1, 0;
            armor._r2 << 1, 0, 0,
                    0, cp, -sp,
                    0, sp, cp;
            armor._r3 << cy, -sy, 0,
                    sy, cy, 0,
                    0, 0, 1;
            // 计算装甲板中心点在世界坐标系下的坐标,以及装甲板的旋转矩阵
            armor.rotation = armor._r3 * armor._r2 * armor._r1 * armor._rmat;
#ifdef DEBUG_PARAMETERS
            // 将相机偏移值存入向量
            _cam_bias << float (cv::getTrackbarPos("camera_bias_z", "parameters center")), float (cv::getTrackbarPos("camera_bias_y", "parameters center")), 0;
#endif
            armor.position = armor._r3 * armor._r2 * (armor._r1 * armor._translation + _cam_bias) / 1000;
            return true;
        }

        bool GetTransformation(Flabellum &flab, float g_pitch, float g_yaw, const double &dt) {
            cv::Mat rvec, tvec;
            if(flab.points2d[0].y>flab.points2d[1].y){std::swap(flab.points2d[0], flab.points2d[1]);}
            if(flab.points2d[3].y>flab.points2d[2].y){std::swap(flab.points2d[3], flab.points2d[2]);}
            cv::solvePnP(buff_armor_points_,
                            flab.points2d,
                            camera_matrix_,
                            dist_coeffs_,
                            rvec,
                            tvec,
                            false,
                            cv::SOLVEPNP_IPPE);
             // 构造变换矩阵
            flab._translation << tvec.at<double>(0, 0), tvec.at<double>(1, 0), tvec.at<double>(2, 0);
            Eigen::AngleAxisd rot_angle(cv::norm(rvec),
                                        Eigen::Vector3d(rvec.at<double>(0, 0),
                                                        rvec.at<double>(1, 0),
                                                        rvec.at<double>(2, 0)));
            flab._rmat = rot_angle.matrix();
            float sp = std::sin(g_pitch), cp = std::cos(g_pitch), sy = std::sin(g_yaw), cy = std::cos(g_yaw);
            flab._r1 << 1, 0, 0,
                    0, 0, 1,
                    0, -1, 0;
            flab._r2 << 1, 0, 0,
                    0, cp, -sp,
                    0, sp, cp;
            flab._r3 << cy, -sy, 0,
                    sy, cy, 0,
                    0, 0, 1;
            // 计算装甲板中心点在世界坐标系下的坐标,以及装甲板的旋转矩阵
            flab.rotation = flab._r3 * flab._r2 * flab._r1 * flab._rmat;
#ifdef DEBUG_PARAMETERS
            // 将相机偏移值存入向量
            _cam_bias << float (cv::getTrackbarPos("camera_bias_z", "parameters center")), float (cv::getTrackbarPos("camera_bias_y", "parameters center")), 0;
#endif
            flab.position = flab._r3 * flab._r2 * (flab._r1 * flab._translation + _cam_bias) / 1000;
            return true;
        }

        /**
         * @brief 实现可视化
         * @param curr_tracker
         * @param radius_1 读取到的装甲板和其相对的装甲板的距离
         * @param radius_2
         * @return
         */
        void ArmorVisualization(const Tracker &curr_tracker, const TargetInfo &target)
        {
            cv::Mat visualization(1000, 1000, CV_8UC3, cv::Scalar(0, 0, 0));                                                                                                      //可视化界面
            cv::putText(visualization, "radius_1:" + std::to_string(target.radius_1), cv::Point(100, 100), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);//显示距离
            cv::putText(visualization, "radius_2:" + std::to_string(target.radius_2), cv::Point(100, 150), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);//显示距离
            cv::putText(visualization, "yaw" + std::to_string(angles::to_degrees(target.yaw)), cv::Point(100, 200), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
            cv::putText(visualization, "vx: " + std::to_string(target.vx), cv::Point(100, 400), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);      //显示w_yaw
            cv::putText(visualization, "x: " + std::to_string(target.x), cv::Point(100, 350), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);        //显示w_yaw
            cv::putText(visualization, "vy: " + std::to_string(target.vy), cv::Point(100, 500), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);      //显示w_yaw
            cv::putText(visualization, "y: " + std::to_string(target.y), cv::Point(100, 450), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);        //显示w_yaw
            cv::putText(visualization, "vz: " + std::to_string(target.vz), cv::Point(100, 600), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);      //显示w_yaw
            cv::putText(visualization, "z: " + std::to_string(target.z), cv::Point(100, 550), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);        //显示w_yaw

            cv::putText(visualization, "w_yaw: " + std::to_string(target.w_yaw), cv::Point(100, 250), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);//显示w_yaw

            // write the tracker state
            std::string tracker_state;
            switch (curr_tracker.tracker_state)
            {
                case Tracker::TRACKING:
                    tracker_state = "TRACKING";
                    break;
                case Tracker::LOST:
                    tracker_state = "LOST";
                    break;
                case Tracker::DETECTING:
                    tracker_state = "DETECTING";
                    break;
                case Tracker::TEMP_LOST:
                    tracker_state = "TEMP_LOST";
                    break;
            }
            cv::putText(visualization, "tracker state:" + tracker_state, cv::Point(100, 300), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
            cv::putText(visualization, "tracker_armor_sum" + std::to_string((int) curr_tracker.tracked_armors_num), cv::Point(100, 325), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
            float bili = 0.05;
            float radius_1 = 2 * target.radius_1 * 1000 * bili;
            float radius_2 = 2 * target.radius_2 * 1000 * bili;
            float t_x = target.x * 1000 * bili;
            float t_y = target.y * 1000 * bili;
            cv::arrowedLine(visualization,
                            cv::Point(0, 500), cv::Point(1000, 500),
                            cv::Scalar(255, 0, 0),
                            1, 8, 0, 0.01);//世界坐标系x轴 蓝色
            cv::arrowedLine(visualization,
                            cv::Point(500, 1000), cv::Point(500, 0),
                            cv::Scalar(0, 255, 255),
                            1, 8, 0, 0.01);//世界坐标系y轴 黄色
            Eigen::Vector3d x_c(50 / bili, 0, 0);
            Eigen::Vector3d y_c(0, 50 / bili, 0);
            const auto armor = curr_tracker.tracked_armor;
            cv::putText(visualization, "xa: " + std::to_string(armor.position(0)), cv::Point(100, 700), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
            cv::putText(visualization, "ya: " + std::to_string(armor.position(1)), cv::Point(100, 750), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
            cv::putText(visualization, "za: " + std::to_string(armor.position(2)), cv::Point(100, 800), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
            double dis_armor_to_origin = armor.position.norm();
            double dis_center_to_origin = sqrt(pow(target.x,2) + pow(target.y, 2) + pow(target.z, 2));
            cv::putText(visualization, "armor_dis: " + std::to_string(dis_armor_to_origin), cv::Point(800, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
            cv::putText(visualization, "center_dis: " + std::to_string(dis_center_to_origin), cv::Point(800, 100), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
            Eigen::Vector3d x_w = armor._r3 * armor._r2 * (armor._r1 * (armor._rmat * x_c + armor._translation) + _cam_bias);
            Eigen::Vector3d z_w = armor._r3 * armor._r2 * (armor._r1 * (armor._rmat * y_c + armor._translation) + _cam_bias);
            Eigen::Vector3d center_w = armor._r3 * armor._r2 * (armor._r1 * armor._translation + _cam_bias);
//            std::cout<<"z_w-center_w"<<std::endl;
//            std::cout<<z_w-center_w<<std::endl;
            //            arrowedLine(visualization,
            //                        cv::Point((int) (500 + center_w(0) * bili), (int) (500 - center_w(1) * bili)),
            //                        cv::Point((int) (x_w(0) * bili + 500), (int) (500 - x_w(1) * bili)),
            //                        cv::Scalar(255, 0, 0),
            //                        2, 8, 0, 0.3);//装甲板的x轴
            //            arrowedLine(visualization,
            //                        cv::Point(int(500 + center_w(0) * bili), (int) (500 - center_w(1) * bili)),
            //                        cv::Point((int) (z_w(0) * bili + 500), (int) (500 - z_w(1) * bili)),
            //                        cv::Scalar(0, 255, 0),
            //                        2, 8, 0, 0.3);//装甲板的z轴
            double rad_z = atan2((z_w(1) - center_w(1)), (z_w(0) - center_w(0)));
            double rad_x = atan2((x_w(1) - center_w(1)), (x_w(0) - center_w(0)));
            cv::putText(visualization, "yaw: " + std::to_string(angles::to_degrees(rad_z)), cv::Point(100, 850), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);

            //            line(visualization,
            //                 cv::Point((int) (500 + center_w(0) * bili), (int) (500 - center_w(1) * bili)),
            //                 cv::Point((int) (500 + center_w(0) * bili + radius_1 * cos(rad_z)),
            //                           (int) (500 - center_w(1) * bili - radius_1 * sin(rad_z))),
            //                 cv::Scalar(255, 255, 255),
            //                 2);//根据半径画出车
            //            line(visualization,
            //                 cv::Point((int) (500 + center_w(0) * bili + radius_1 * cos(rad_z) / 2 - radius_2 * cos(rad_x) / 2),
            //                           (int) (500 - center_w(1) * bili - radius_1 * sin(rad_z) / 2 + radius_2 * sin(rad_x) / 2)),
            //                 cv::Point((int) (500 + center_w(0) * bili + radius_1 * cos(rad_z) / 2 + radius_2 * cos(rad_x) / 2),
            //                           (int) (500 - center_w(1) * bili - radius_1 * sin(rad_z) / 2 - radius_2 * sin(rad_x) / 2)),
            //                 cv::Scalar(255, 255, 255),
            //                 2);//根据半径画出车
//            circle(visualization,
//                   cv::Point((int) (500 + center_w(0) * bili + radius_1 * cos(rad_z) / 2),
//                             (int) (500 - center_w(1) * bili - radius_1 * sin(rad_z) / 2)),
//                   3,
//                   cv::Scalar(0, 255, 0), -1);//车的中心
            circle(visualization,
                   cv::Point(int(500 + center_w(0) * bili), (int) (500 - center_w(1) * bili)),
                   3,
                   cv::Scalar(0, 255, 0), -1);//装甲板的中心
            ///111
            float yaw1=target.yaw;
            float fx = target.x ;
            float fy = target.y ;
            float fz = target.z ;
            cv::Point3f armor1,armor2,armor3,armor4;
            armor1.x=fx-target.radius_1*cos(yaw1);
            armor1.y=fy-target.radius_1*sin(yaw1);
            armor1.z=fz;
            armor2.x=fx+target.radius_1*cos(yaw1);
            armor2.y=fy+target.radius_1*sin(yaw1);
            armor2.z=fz;
            armor3.x=fx-target.radius_2*sin(yaw1);
            armor3.y=fy+target.radius_2*cos(yaw1);
            armor3.z=fz+target.dz;
            armor4.x=fx+target.radius_2*sin(yaw1);
            armor4.y=fy-target.radius_2*cos(yaw1);
            armor4.z=fz+target.dz;
            std::vector<cv::Point3f> armors;
            armors.push_back(armor1);
            armors.push_back(armor2);
            armors.push_back(armor3);
            armors.push_back(armor4);
            sort(armors.begin(), armors.end(), Compensator::cmp);
            double rad0=atan2(armors[0].y,armors[0].x);
            double rad1=atan2(armors[1].y,armors[1].x);
            double rad=atan2(target.y,target.x);
            if(abs(angles::shortest_angular_distance(rad0,rad))<abs(angles::shortest_angular_distance(rad1,rad)))
            {
                fx=armors[0].x;
                fy=armors[0].y;
                fz=armors[0].z;
            }
            else
            {
                fx=armors[1].x;
                fy=armors[1].y;
                fz=armors[1].z;
            }
            circle(visualization,
                   cv::Point((int) (500 + fx*1000*bili),
                             (int) (500 - fy*1000*bili)),
                   3,
                   cv::Scalar(255, 255, 255), -1);
            ///111
            line(visualization,
                 cv::Point((int) (500 + t_x - radius_1 * cos(target.yaw) / 2),
                           (int) (500 - t_y + radius_1 * sin(target.yaw) / 2)),
                 cv::Point((int) (500 + t_x + radius_1 * cos(target.yaw) / 2),
                           (int) (500 - t_y - radius_1 * sin(target.yaw) / 2)),
                 cv::Scalar(0, 0, 255),
                 2);//根据半径画出车
            line(visualization,
                 cv::Point((int) (500 + t_x - radius_2 * sin(target.yaw) / 2),
                           (int) (500 - t_y - radius_2 * cos(target.yaw) / 2)),
                 cv::Point((int) (500 + t_x + radius_2 * sin(target.yaw) / 2),
                           (int) (500 - t_y + radius_2 * cos(target.yaw) / 2)),
                 cv::Scalar(0, 0, 255),
                 2);//根据半径画出车
            circle(visualization,
                   cv::Point((int) (500 + t_x),
                             (int) (500 - t_y)),
                   3,
                   cv::Scalar(0, 0, 255), -1);//车的中心
            cv::namedWindow("visualization", cv::WINDOW_AUTOSIZE);
            cv::imshow("visualization", visualization);
        }

    private:
        cv::Mat camera_matrix_;  // 相机矩阵
        cv::Mat dist_coeffs_;  // 焦距系数

        // 小装甲板的灯条组成矩形的宽度，单位：毫米
        static constexpr float SMALL_ARMOR_WIDTH = 135;
        // 小装甲板的灯条组成矩形的高度，单位：毫米
        static constexpr float SMALL_ARMOR_HEIGHT = 55;
        // 大装甲板的灯条组成矩形的宽度，单位：毫米
        static constexpr float LARGE_ARMOR_WIDTH = 225;
        // 大装甲板的高度，单位：毫米
        static constexpr float LARGE_ARMOR_HEIGHT = 55;

        static constexpr float BUFF_ARMOR_HEIGHT = 380;
        static constexpr float BUFF_ARMOR_WIDTH = 380;

        // 三维空间中护甲的四个顶点
        std::vector<cv::Point3f> small_armor_points_;
        // 三维空间中护甲的四个顶点
        std::vector<cv::Point3f> large_armor_points_;
        std::vector<cv::Point3f> buff_armor_points_;
        Eigen::Vector3d _cam_bias;
        double angle = 0.2 * CV_PI;  // 角度，单位：弧度
        float lastr=0.14;  // 上次测量的距离
        float radius=0.24;  // 半径
        cv::Point2f ccc = cv::Point2f(3,3);
        std::vector<cv::Point3f> armorss;
        std::vector<cv::Point3f> armorss1;

    public:
        //@property
        Eigen::Vector3d get_cam_bias(){
            return this->_cam_bias;
        }
    };

}// namespace hnurm

#endif// ARMOR_DETECTOR_PNP_SOLVER_HPP_
